
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rcprotocol_sbus.c
  * @author     baiyang
  * @date       2022-6-11
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rcprotocol_sbus.h"

#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
#define SBUS_FRAME_SIZE     25
#define SBUS_INPUT_CHANNELS 16
#define SBUS_FLAGS_BYTE     23
#define SBUS_FAILSAFE_BIT   3
#define SBUS_FRAMELOST_BIT  2

/* define range mapping here, -+100% -> 1000..2000 */
#define SBUS_RANGE_MIN 200
#define SBUS_RANGE_MAX 1800
#define SBUS_RANGE_RANGE (SBUS_RANGE_MAX - SBUS_RANGE_MIN)

#define SBUS_TARGET_MIN 1000
#define SBUS_TARGET_MAX 2000
#define SBUS_TARGET_RANGE (SBUS_TARGET_MAX - SBUS_TARGET_MIN)

// this is 875
#define SBUS_SCALE_OFFSET (SBUS_TARGET_MIN - ((SBUS_TARGET_RANGE * SBUS_RANGE_MIN / SBUS_RANGE_RANGE)))

#ifndef HAL_SBUS_FRAME_GAP
#define HAL_SBUS_FRAME_GAP 2000U
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void process_pulse(rcprotocol_backend_t backend, uint32_t width_s0, uint32_t width_s1);
static void process_byte(rcprotocol_backend_t backend, uint8_t b, uint32_t baudrate);
static bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
                                     bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values);
static void _process_byte(rcprotocol_sbus_t sbus, uint32_t timestamp_us, uint8_t b);
/*----------------------------------variable----------------------------------*/
static struct rcprotocol_backend_ops sbus_ops = {.rcprotocol_backend_destructor = NULL,
                                                 .process_pulse = process_pulse,
                                                 .process_byte = process_byte,
                                                 .process_handshake = NULL,
                                                 .start_bind = NULL,
                                                 .update = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rcprotocol_sbus_ctor(rcprotocol_sbus_t sbus, bool _inverted, uint32_t configured_baud)
{
    rcprotocol_backend_ctor(&sbus->backend, &sbus_ops);
    sbus->inverted = _inverted;
    softserial_ctor(&sbus->ss, configured_baud, SERIAL_CONFIG_8E2I);
}

rcprotocol_backend_t rcprotocol_sbus_probe(bool _inverted, uint32_t configured_baud)
{
    rcprotocol_sbus_t sbus = (rcprotocol_sbus_t)rt_malloc(sizeof(struct rcprotocol_sbus));

    if (sbus) {
        rt_memset(sbus, 0, sizeof(struct rcprotocol_sbus));
        rcprotocol_sbus_ctor(sbus, _inverted, configured_baud);

        return (rcprotocol_backend_t)sbus;
    }

    return NULL;
}

/*
  process a SBUS input pulse of the given width
 */
static void process_pulse(rcprotocol_backend_t backend, uint32_t width_s0, uint32_t width_s1)
{
    rcprotocol_sbus_t sbus = (rcprotocol_sbus_t)backend;

    uint32_t w0 = width_s0;
    uint32_t w1 = width_s1;
    if (sbus->inverted) {
        w0 = sbus->saved_width;
        w1 = width_s0;
        sbus->saved_width = width_s1;
    }
    uint8_t b;
    if (softserial_process_pulse(&sbus->ss, w0, w1, &b)) {
        _process_byte(sbus, softserial_get_byte_timestamp_us(&sbus->ss), b);
    }
}

// support byte input
static void process_byte(rcprotocol_backend_t backend, uint8_t b, uint32_t baudrate)
{
    rcprotocol_sbus_t sbus = (rcprotocol_sbus_t)backend;

    // note that if we're here we're not actually using SoftSerial,
    // but it does record our configured baud rate:
    if (baudrate != softserial_baud(&sbus->ss)) {
        return;
    }
    _process_byte(sbus, time_micros(), b);
}

// decode a full SBUS frame
static bool sbus_decode(const uint8_t frame[25], uint16_t *values, uint16_t *num_values,
                                     bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{
    /* check frame boundary markers to avoid out-of-sync cases */
    if ((frame[0] != 0x0f)) {
        return false;
    }

    switch (frame[24]) {
    case 0x00:
        /* this is S.BUS 1 */
        break;
    case 0x03:
        /* S.BUS 2 SLOT0: RX battery and external voltage */
        break;
    case 0x83:
        /* S.BUS 2 SLOT1 */
        break;
    case 0x43:
    case 0xC3:
    case 0x23:
    case 0xA3:
    case 0x63:
    case 0xE3:
        break;
    default:
        /* we expect one of the bits above, but there are some we don't know yet */
        break;
    }

    uint16_t chancount = SBUS_INPUT_CHANNELS;

    rcprotocol_backend_decode_11bit_channels((const uint8_t*)(&frame[1]), SBUS_INPUT_CHANNELS, values,
        SBUS_TARGET_RANGE, SBUS_RANGE_RANGE, SBUS_SCALE_OFFSET);

    /* decode switch channels if data fields are wide enough */
    if (max_values > 17 && SBUS_INPUT_CHANNELS > 15) {
        chancount = 18;

        /* channel 17 (index 16) */
        values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0))?1998:998;
        /* channel 18 (index 17) */
        values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1))?1998:998;
    }

    /* note the number of channels decoded */
    *num_values = chancount;

    /* decode and handle failsafe and frame-lost flags */
    if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
        /* report that we failed to read anything valid off the receiver */
        *sbus_failsafe = true;
        *sbus_frame_drop = true;
    } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
        /* set a special warning flag
         *
         * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
         * condition as fail-safe greatly reduces the reliability and range of the radio link,
         * e.g. by prematurely issuing return-to-launch!!! */

        *sbus_failsafe = false;
        *sbus_frame_drop = true;
    } else {
        *sbus_failsafe = false;
        *sbus_frame_drop = false;
    }

    return true;
}

// support byte input
static void _process_byte(rcprotocol_sbus_t sbus, uint32_t timestamp_us, uint8_t b)
{
    const bool have_frame_gap = (timestamp_us - sbus->byte_input.last_byte_us >= HAL_SBUS_FRAME_GAP);
    sbus->byte_input.last_byte_us = timestamp_us;

    if (have_frame_gap) {
        // if we have a frame gap then this must be the start of a new
        // frame
        sbus->byte_input.ofs = 0;
    }
    if (b != 0x0F && sbus->byte_input.ofs == 0) {
        // definately not SBUS, missing header byte
        return;
    }
    if (sbus->byte_input.ofs == 0 && !have_frame_gap) {
        // must have a frame gap before the start of a new SBUS frame
        return;
    }

    sbus->byte_input.buf[sbus->byte_input.ofs++] = b;

    if (sbus->byte_input.ofs == sizeof(sbus->byte_input.buf)) {
        rcprotocol_backend_log_data(RCP_SBUS, timestamp_us, sbus->byte_input.buf, sbus->byte_input.ofs);
        uint16_t values[SBUS_INPUT_CHANNELS];
        uint16_t num_values=0;
        bool sbus_failsafe = false;
        bool sbus_frame_drop = false;
        if (sbus_decode(sbus->byte_input.buf, values, &num_values,
                     &sbus_failsafe, &sbus_frame_drop, SBUS_INPUT_CHANNELS) &&
         num_values >= MIN_RCIN_CHANNELS) {
            rcprotocol_backend_add_input(&sbus->backend, num_values, values, sbus_failsafe, -1, -1);
        }
        sbus->byte_input.ofs = 0;
    }
}

/*------------------------------------test------------------------------------*/


